#include <chrono>
#include <iostream>
#include "planner_core.h"
#include "dynamicvoronoi.h"
#include "graph_search.h"

void astarFindpath(const cv::Mat *msg_image, std::vector<std::pair<int, int>> &msg_points);

void jpsFindpath(const cv::Mat *msg_image, std::vector<std::pair<int, int>> &msg_points)
{
  float eps = 0.001;
  cv::Mat my_map = *msg_image;
  cv::cvtColor(my_map, my_map, cv::COLOR_GRAY2BGR);

  auto Jps_time = std::chrono::high_resolution_clock::now();
  std::shared_ptr<JPS::GraphSearch> graph_search_ = std::make_shared<JPS::GraphSearch>(*msg_image, my_map.cols, my_map.rows, eps, false);
  graph_search_->plan(msg_points[0], msg_points[1], true);

  auto Jend_time = std::chrono::high_resolution_clock::now();
  auto Jduration_path = std::chrono::duration_cast<std::chrono::milliseconds>(Jend_time - Jps_time).count();

  const auto path = graph_search_->getPath();

  cv::circle(my_map, cv::Point(msg_points[0].first, msg_points[0].second), 5, cv::Scalar(0, 255, 0), -1);
  cv::circle(my_map, cv::Point(msg_points[1].first, msg_points[1].second), 5, cv::Scalar(0, 255, 0), -1);

  std::cout << "Jpspath_size: " << path.size() << std::endl;
  for (const auto &it : path){
    cv::Point center(it->x, it->y);
    cv::circle(my_map, center, 2, cv::Scalar(255, 0, 0), -1);
  }

  double distance1 = (msg_points[0].first - msg_points[1].first) * (msg_points[0].first - msg_points[1].first);
  double distance2 = (msg_points[0].second - msg_points[1].second) * (msg_points[0].second - msg_points[1].second);
  double distance3 = std::sqrt(distance1 + distance2) / 20;

  std::string text = "Time: " + std::to_string(Jduration_path) + "ms. " + std::to_string(distance3) + "m.";
  cv::putText(my_map, text, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2);

  // add code
  // astarFindpath(msg_image, msg_points);
  // end
  msg_points.clear();
  cv::namedWindow("jpsmimage");
  cv::imshow("jpsmimage", my_map);
  cv::imwrite("Jpsmap_path.png", my_map);
  cv::waitKey(0);
}


static findPlanner::Planner new_path;
void astarFindpath(const cv::Mat *msg_image, std::vector<std::pair<int, int>> &msg_points)
{
  cv::Mat my_map = *msg_image;
  cv::cvtColor(my_map, my_map, cv::COLOR_GRAY2BGR);

  auto Astart_path = std::chrono::high_resolution_clock::now();
  new_path.initializeLookups(*msg_image);
  std::vector<std::pair<int, int>> path_pont;
  new_path.astartfindPath(msg_points[0], msg_points[1], path_pont);

  auto Aend_path = std::chrono::high_resolution_clock::now();
  auto Aduration_path = std::chrono::duration_cast<std::chrono::milliseconds>(Aend_path - Astart_path).count();
  std::cout << "astarFindpath of code execution time: " << Aduration_path << " ms." << std::endl;

  cv::circle(my_map, cv::Point(msg_points[0].first, msg_points[0].second), 5, cv::Scalar(0, 255, 0), -1);
  cv::circle(my_map, cv::Point(msg_points[1].first, msg_points[1].second), 5, cv::Scalar(0, 255, 0), -1);

  std::cout << "path_pont_size: " << path_pont.size() << std::endl;
  for (const auto &ptr : path_pont){
    cv::Point center(ptr.first, ptr.second);
    cv::circle(my_map, center, 1, cv::Scalar(255, 0, 0), -1);
  }

  double distance1 = (msg_points[0].first - msg_points[1].first) * (msg_points[0].first - msg_points[1].first);
  double distance2 = (msg_points[0].second - msg_points[1].second) * (msg_points[0].second - msg_points[1].second);
  double distance3 = std::sqrt(distance1 + distance2) / 20;

  std::string text = "Time: " + std::to_string(Aduration_path) + "ms. " + std::to_string(distance3) + "m.";
  cv::putText(my_map, text, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2);

  msg_points.clear();
  cv::namedWindow("astarmimage");
  cv::imshow("astarmimage", my_map);
  cv::imwrite("astartmap_path.png", my_map);
  cv::waitKey(0);
}

static findPlanner::mpointData mSampling_points;  // voronoi顶点数据, 后面可以用函数指针代替
static findPlanner::mpointData mObstacle_points; // 障碍物点数据, 后面可以用函数指针代替
void voronoiFindpath(const cv::Mat *msg_image, std::vector<std::pair<int, int>> &msg_points)
{
  cv::Mat my_map = *msg_image;
  cv::cvtColor(my_map, my_map, cv::COLOR_GRAY2BGR);

  auto start_path = std::chrono::high_resolution_clock::now();
  new_path.initializeLookups(*msg_image);
  std::vector<std::pair<int, int>> path_pont1;
  std::vector<std::pair<int, int>> path_pont2;
  std::vector<std::pair<int, int>> path_pont3;

  bool res1 = new_path.findPath(msg_points[0], msg_points[1], path_pont1, 0);
  bool res2 = new_path.findPath(msg_points[1], msg_points[0], path_pont3, 0);

  if (path_pont1.empty())
    path_pont1.emplace_back(msg_points[0]);
  if (path_pont3.empty())
    path_pont3.emplace_back(msg_points[1]);

  bool res3 = new_path.findPath(path_pont1.back(), path_pont3.back(), path_pont2, 1);

  if(res1 && res2 && res3){
    path_pont1.insert(path_pont1.end(), path_pont2.begin(), path_pont2.end());
    path_pont1.insert(path_pont1.end(), path_pont3.begin(), path_pont3.end());
  }
  else{
    std::cout << "No path found in search!" << std::endl;
    path_pont1.insert(path_pont1.end(), path_pont2.begin(), path_pont2.end());
  }

  auto end_path = std::chrono::high_resolution_clock::now();
  auto duration_path = std::chrono::duration_cast<std::chrono::milliseconds>(end_path - start_path).count();
  std::cout << "voronoiFindpath_ of code execution time: " << duration_path << " ms." << std::endl;

  // 1.绘制voronoi地图
  for (const auto &point : mSampling_points){
    cv::Point center(point.first, point.second);
    cv::circle(my_map, center, 0.1, cv::Scalar(0, 0, 200), -1);
  }

  // 2.绘制起点和终点
  cv::circle(my_map, cv::Point(msg_points[0].first, msg_points[0].second), 5, cv::Scalar(255, 0, 0), -1);
  cv::circle(my_map, cv::Point(msg_points[1].first, msg_points[1].second), 5, cv::Scalar(255, 0, 0), -1);

  // 3.绘制路径
  std::cout << "path_pont_size: " << path_pont1.size() << std::endl;
  for (const auto &ptr : path_pont1){
    cv::Point center(ptr.first, ptr.second);
    cv::circle(my_map, center, 1, cv::Scalar(0, 255, 0), -1);
  }

  double distance1 = (msg_points[0].first - msg_points[1].first) * (msg_points[0].first - msg_points[1].first);
  double distance2 = (msg_points[0].second - msg_points[1].second) * (msg_points[0].second - msg_points[1].second);
  double distance3 = std::sqrt(distance1 + distance2) / 20;

  std::string text = "Time: " + std::to_string(duration_path) + "ms. " + std::to_string(distance3) + "m.";
  cv::putText(my_map, text, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2);

  // add code
  // astarFindpath(msg_image, msg_points);
  // end

  msg_points.clear();
  cv::namedWindow("mimage");
  cv::imshow("mimage", my_map);
  cv::imwrite("voronoi_path.png", my_map);
  cv::waitKey(0);
}


static int search_mode = 0;
void onMouse(int event, int x, int y, int flags, void *userdata)
{
  static std::vector<std::pair<int, int>> points;
  if (event == cv::EVENT_LBUTTONDOWN){
    cv::Mat *mimage = static_cast<cv::Mat *>(userdata);
    if (mimage == nullptr || mimage->empty()){
      std::cerr << "Image is empty." << std::endl;
      return;
    }
    int gridValue = static_cast<int>(mimage->at<uchar>(y, x));
    std::cout << "Grid value at (" << x << ", " << y << "): " << gridValue << std::endl;

    if (gridValue == 255)
      points.emplace_back(std::make_pair(x, y));
    if (points.size() == 2)
    {
      if (search_mode == 0){
        std::cout << "Astar of ordinary mode: " << search_mode << std::endl;
        astarFindpath(mimage, points);
      }
      else if (search_mode == 1){
        std::cout << "Jps of ordinary mode: " << search_mode << std::endl;
        jpsFindpath(mimage, points);
      }
      else{
        std::cout << "voronoi mode: " << search_mode << std::endl;
        voronoiFindpath(mimage, points);
      }
    }
  }
}


void loadPGM(std::istream &is, int *sizeX, int *sizeY, bool ***map)
{
  std::string tag;
  is >> tag;
  if (tag != "P5"){
    std::cerr << "Awaiting 'P5' in pgm header, found " << tag << std::endl;
    exit(-1);
  }

  while (is.peek() == ' ' || is.peek() == '\n')
    is.ignore();
  while (is.peek() == '#')
    is.ignore(255, '\n');
  is >> *sizeX;
  while (is.peek() == '#')
    is.ignore(255, '\n');
  is >> *sizeY;
  while (is.peek() == '#')
    is.ignore(255, '\n');
  is >> tag;

  if (tag != "255"){
    std::cerr << "Awaiting '255' in pgm header, found " << tag << std::endl;
    exit(-1);
  }
  is.ignore(255, '\n');

  *map = new bool *[*sizeX];
  for (int x = 0; x < *sizeX; x++){
    (*map)[x] = new bool[*sizeY];
  }
  for (int y = *sizeY - 1; y >= 0; y--)
  {
    for (int x = 0; x < *sizeX; x++){
      int c = is.get();
      if ((double)c < 255 - 255 * 0.2)  // 204  小于这个值的栅格均为障碍物栅格
        (*map)[x][y] = true;  // cell is occupied
      else
        (*map)[x][y] = false; // cell is free
      if (!is.good()){
        std::cerr << "Error reading pgm map.\n";
        exit(-1);
      }
    }
  }
  return;
}


int main(int argc, char **argv)
{
  if (argc < 3 || argc > 4){
    std::cerr << "usage: " << argv[0] << " <pgm map>\n";
    exit(-1);
  }
  search_mode = atoi(argv[2]);  // 修改路径搜索模式
  std::cout << "argv: " << argv[2] << " " << argv[2][0] << " search_mode_: " << search_mode << std::endl;

  std::string my_path = argv[1];
  std::ifstream is_file(my_path.c_str());
  if (is_file.good())
  {
    cv::Mat image = cv::imread(my_path.c_str(), cv::IMREAD_GRAYSCALE);
    if (image.empty()){
      std::cerr << "Failed to load image." << std::endl;
    }

    bool **grid = nullptr;
    int msg_w, msg_h;
    loadPGM(is_file, &msg_w, &msg_h, &grid);

    auto start = std::chrono::high_resolution_clock::now();
    DynamicVoronoi dynamicmap;
    dynamicmap.initializeMap(msg_w, msg_h, grid);
    dynamicmap.update();
    dynamicmap.prune();

    // add code *************
    // int numPoints = 10 + msg_w * msg_h * 0.0005;
    // std::vector<IntPoint> newObstacles; // 障碍物点集合
    // for (int i = 0; i < numPoints; i++){
    //   double x = 2 + rand() / (double)RAND_MAX * (msg_w - 4);
    //   double y = 2 + rand() / (double)RAND_MAX * (msg_h - 4);
    //   newObstacles.push_back(IntPoint(x, y));
    // }
    // dynamicmap.exchangeObstacles(newObstacles); // 更新这一次的新障碍，去除上一次的旧障碍(但底图的障碍物坐标点不会变)
    // dynamicmap.update();
    // dynamicmap.prune();
    // end ******************
    
    std::cout << "msg_wh: " << msg_w << " " << msg_h << std::endl;
    auto end = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "Dynamicmap of code execution time: " << duration << " ms." << std::endl;

    std::string mappath = my_path + ".ppm";
    std::vector<std::pair<int, int>> new_vertex = dynamicmap.visualize(mappath.c_str());
    for (const auto &point : new_vertex){
      mSampling_points.insert(point);
    }

    std::cout << "mSampling_points_size: " << mSampling_points.size() << std::endl;
    new_path.initializePointData(mSampling_points);

    cv::namedWindow("Path Map");
    cv::imshow("Path Map", image);
    cv::setMouseCallback("Path Map", onMouse, &image);
    cv::waitKey(0);
  }
  new_path.removeMap();
  return 0;
}







